﻿using System;
using System.Collections.Generic;
using System.Collections;
using System.Drawing;

namespace MonoPhysics
{
    /// <summary>
    /// Moteur physique.
    /// </summary>
    public static class ImpulseSolver
    {
        private const float GRAVITY_G = 9.81f;
        public static bool AllowRotations = true;
        public static void ApplyImpulse(Edge c)
        {
            if (c.collisions != null)
            {
                RigidBody cible = (c.ue1.M == 0) ? c.ue2 : c.ue1;
                foreach (Collision col in c.collisions)
                {
                    float j;
                    Vector norm = col.collisionNormal.direction();
                    //Console.Write(norm.Magnitude);
                    Vector rVel = col.relativeVelocity;
                    Vector v2CollisionPoint = new Vector(new Vector(col.collisionPoint) - c.ue2.Pos);
                    Vector v1CollisionPoint = new Vector(new Vector(col.collisionPoint) - c.ue1.Pos);
                    //cnormInvert.Y = -cnormInvert.Y;
                    float VVello = -Math.Abs(rVel.dot(norm));
                    ////Console.WriteLine(VVello.ToString() + " <<<>>> " + col.relativeVelocity.dot(col.collisionNormal).ToString());
                    float CNorm = norm.dot(norm);
                    float Numerateur = -((1.0f + Math.Max(0.1f, 0/*c.ue1.Restitution, c.ue2.Restitution*/)) * (VVello));
                    float Moments = norm.dot((v2CollisionPoint.cross(norm) * c.ue2._invI).cross(v2CollisionPoint)) +
                                     norm.dot((v1CollisionPoint.cross(norm) * c.ue1._invI).cross(v1CollisionPoint));
                    float Denominateur = (((AllowRotations && !c.ue1.PhysicStats.IgnoreRotation && !c.ue2.PhysicStats.IgnoreRotation) ? Moments : 0) +
                                          (c.ue1._invM +
                                           c.ue2._invM)
                                         );
                    j = (Denominateur == 0) ? 0 : Numerateur / Denominateur;

                    // Frottements
                    //if (c.ue1.M != 0)
                    //{
                    PhysicContext pc = cible.PhysicStats;
                    //c.collisions[0].collisionNormal.X = -col.collisionNormal.X;
                    Vector nv = pc.V;
                    cible.PhysicStats.Forces.Add(new Force(j * norm, (c.ue1.M == 0) ? v2CollisionPoint : v1CollisionPoint));
                    //}
                    /*if (c.ue2.M != 0)
                    {
                        PhysicContext pc = c.ue2.PhysicStats;
                        Vector nv = pc.V;
                        c.ue2.PhysicStats.Forces.Add(new Force(-j * norm, v2CollisionPoint));
                    }*/
                }
            }
        }

        public static void applyGravity(RigidBody e)
        {
            if (e.M != 0)
            {
                PhysicContext pc = e.PhysicStats;
                Vector vit = e.PhysicStats.V;
                vit.Y += GRAVITY_G;
                pc.V = vit;
                e.PhysicStats = pc;
            }
        }


        public static void ExecObjectforces(RigidBody e, float dt)
        {
            List<Force> fs = e.PhysicStats.Forces;
            List<Force> es = e.PhysicStats.ExtraForces;
            Vector Accel = new Vector();
            Vector Angccel = new Vector();
            //e.CollisionPoints.Clear();
            Force pf = e.PhysicStats.PersistantForce;
            int l = e.PhysicStats.Forces.Count + es.Count + ((pf.force.X == 0 && pf.force.Y == 0) ? 0 : 1);
            if (l < 1)
                l = 1;
            if (fs != null)
            {
                if (e.M != 0)
                {
                    if (fs.Count > 0)
                    {
                        for (int i = 0; i < fs.Count; i++)
                        {
                            Force v = fs[i];
                            Accel += v.force / l;
                            Angccel += v.location.cross(v.force) * e._invI / l;
                        }
                    }
                    int j = es.Count / 2;
                    for (int i = 0; i < j * 2; i++)
                    {
                        Force v = es[i];
                        Accel += v.force / j;
                        Angccel += v.location.cross(v.force) * e._invI / j;
                    }
                    Accel += e.PhysicStats.PersistantForce.force;
                    Angccel += e.PhysicStats.PersistantForce.location.cross(e.PhysicStats.PersistantForce.force);
                    Accel.X *= e._invM;
                    Accel.Y *= e._invM;
                    Accel.Z *= e._invM;
                    Vector vit = (e.PhysicStats.IgnoreTranslation) ? new Vector() : e.PhysicStats.V + Accel;
                    float AngularVelocity = (AllowRotations) ? e.PhysicStats.AngularVitess + Angccel.Z : 0;
                    PhysicContext pc = e.PhysicStats;
                    if (!pc.IgnoreRotation)
                        pc.AngularVitess = (Math.Abs(AngularVelocity) > 0.5f) ? ((AngularVelocity < 0) ? -0.5f : 0.5f) : AngularVelocity;
                    if (vit.X > 0)
                    {
                        if (vit.X > pc.MaxVitessX && pc.MaxVitessX != 0)
                            vit.X = pc.MaxVitessX;
                    }
                    else
                    {
                        if (-vit.X > pc.MaxVitessX && pc.MaxVitessX != 0)
                            vit.X = -pc.MaxVitessX;
                    }
                    if (vit.Y > 0)
                    {
                        if (vit.Y > pc.MaxVitessY && pc.MaxVitessY != 0)
                            vit.Y = pc.MaxVitessY;
                    }
                    else
                    {
                        if (vit.Y < pc.MinVitessY && pc.MinVitessY != 0)
                            vit.Y = pc.MinVitessY;
                    }//*/
                    pc.V = vit;
                    //pc.V = (magn > 500) ? vit.direction() * 500 : vit;
                    if (true)//magn > 25.0f) // mettre 0 pour que les frottements fonctionent sur les plateformes!
                    {
                        Vector newPosDim = new Vector(pc.V.X * dt + e.Pos.X, pc.V.Y * dt + e.Pos.Y, 0);
                        e.Pos = newPosDim;
                    }
                    pc.PersistantForce = new Force();
                    e.PhysicStats = pc;
                    //if (Math.Abs(e.PhysicStats.AngularVitess) > 0.01f)
                    if (!pc.IgnoreRotation)
                        e.RotationGrad += e.PhysicStats.AngularVitess * dt;
                    e.PhysicStats.Forces.Clear();
                    e.PhysicStats.ExtraForces.Clear();
                    //Console.WriteLine(pc.V.ToString());

                    lastMovedObjects.Add(e);
                    //Console.WriteLine("////////////////////////////////////"+(t2-t1).ToString());
                }
            }
        }

        public static void UpdateVelocities(RigidBody e, float dt)
        {
            List<Force> fs = e.PhysicStats.Forces;
            List<Force> es = e.PhysicStats.ExtraForces;
            Vector Accel = new Vector();
            Vector Angccel = new Vector();
            e.CollisionPoints.Clear();
            int l = e.PhysicStats.Forces.Count - 1 + es.Count + ((e.PhysicStats.PersistantForce.force.isNull()) ? 0 : 1);
            if (l < 1)
                l = 1;
            if (fs != null)
            {
                if (e.M != 0)
                {
                    if (fs.Count > 0)
                    {
                        Accel += fs[0].force;
                        for (int i = 1; i < fs.Count; i++)
                        {
                            Force v = fs[i];
                            Accel += v.force / l;
                            Angccel += v.location.cross(v.force) * e._invI / l;
                        }
                        int j = es.Count / 2;
                        for (int i = 0; i < j * 2; i++)
                        {
                            Force v = es[i];
                            Accel += v.force / j;
                            Angccel += v.location.cross(v.force) * e._invI / j;
                        }
                    }
                    Accel += e.PhysicStats.PersistantForce.force;
                    Angccel += e.PhysicStats.PersistantForce.location.cross(e.PhysicStats.PersistantForce.force);
                    Accel.X *= e._invM;
                    Accel.Y *= e._invM;
                    Accel.Z *= e._invM;
                    Vector vit = (e.PhysicStats.IgnoreTranslation) ? new Vector() : e.PhysicStats.V + Accel;
                    float AngularVelocity = (AllowRotations) ? e.PhysicStats.AngularVitess + Angccel.Z : 0;
                    PhysicContext pc = e.PhysicStats;
                    if (!pc.IgnoreRotation)
                        pc.AngularVitess = AngularVelocity;//(Math.Abs(AngularVelocity) > 0.5) ? ((AngularVelocity < 0) ? -0.5f : 0.5f) : AngularVelocity;
                    float magn = vit.Magnitude;
                    if (vit.X > 0)
                    {
                        if (vit.X > pc.MaxVitessX && pc.MaxVitessX != 0)
                            vit.X = pc.MaxVitessX;
                    }
                    else
                    {
                        if (-vit.X > pc.MaxVitessX && pc.MaxVitessX != 0)
                            vit.X = -pc.MaxVitessX;
                    }
                    if (vit.Y > 0)
                    {
                        if (vit.Y > pc.MaxVitessY && pc.MaxVitessY != 0)
                            vit.Y = pc.MaxVitessY;
                    }
                    else
                    {
                        if (vit.Y < pc.MinVitessY && pc.MinVitessY != 0)
                            vit.Y = pc.MinVitessY;
                    }//*/
                    pc.V = vit;
                    e.PhysicStats = pc;
                    //pc.V = (magn > 500) ? vit.direction() * 500 : vit;
                    //if (Math.Abs(e.PhysicStats.AngularVitess) > 0.01f)
                    e.PhysicStats.Forces.Clear();
                    e.PhysicStats.ExtraForces.Clear();
                    ////Console.WriteLine(pc.V.ToString());
                }
            }
        }

        public static void UpdatePositions(RigidBody e, float dt)
        {
            PhysicContext pc = e.PhysicStats;
            if (true)//magn > 25.0f) // mettre 0 pour que les frottements fonctionent sur les plateformes!
            {
                Vector newPosDim = new Vector(pc.V.X * dt + e.Pos.X, pc.V.Y * dt + e.Pos.Y, 0);
                e.Pos = newPosDim;
            }
            e.RotationGrad += e.PhysicStats.AngularVitess * dt;
        }

        private static List<RigidBody> lastMovedObjects = new List<RigidBody>();
        public static void RegisterObjectForBroadPhaseUpdate(RigidBody rb)
        { if (!lastMovedObjects.Contains(rb)) lastMovedObjects.Add(rb); }
        public static List<RigidBody> GetLasMovedObjects()
        { return lastMovedObjects; }
        public static void ClearLasMovedObjects()
        { lastMovedObjects.Clear(); }
    }
}
